import Hobot.GPIO as GPIO


class node_red_car_fsm:

    def __init__(self, left_fw, left_bw, right_fw, right_bw):
        self.state_drive_callback = {}
        self.current_state = "None"
        self.pre_state = "None"

        self.LEFT_PWM_PIN = left_fw
        self.LEFT_GPIO_PIN = left_bw
        self.RIGHT_PWM_PIN = right_fw
        self.RIGHT_GPIO_PIN = right_bw

        self.state_filter = {
            "Stop": None,
            "Forward": None,
            "Backward": None,
            "Left": None,
            "Right": None
        }

        # 设置gpio
        self.gpio_init_setting()

    def gpio_init_setting(self):
        # GPIO初始化
        GPIO.setmode(GPIO.BOARD)

        GPIO.setup(self.LEFT_GPIO_PIN, GPIO.OUT, initial=GPIO.LOW)
        GPIO.setup(self.RIGHT_GPIO_PIN, GPIO.OUT, initial=GPIO.LOW)

        self.left_pwm = GPIO.PWM(self.LEFT_PWM_PIN, 48000)  # PWM频率48kHz
        self.right_pwm = GPIO.PWM(self.RIGHT_PWM_PIN, 48000)

        # 初始状态设置
        self.left_pwm.ChangeDutyCycle(1)
        self.right_pwm.ChangeDutyCycle(1)
        self.left_pwm.start(0)
        self.right_pwm.start(0)

    # 阶段1:实现前向pid控制
    # 阶段2:实现后向pid控制
    # 阶段3:实现指定角度转向

    def set_state(self, state):
        # 过滤重复状态
        if state == self.current_state:
            return 
        # 过滤无效状态
        if state in self.state_filter:
            self.pre_state = self.current_state
            self.current_state = state
            print("状态更新→" + state)
            self.apply_state_action()
    
    
    def apply_state_action(self):
        
        print("TODO")
    
    
    def apply_state_callback(self,state,callback):
        # 验证key是否存在
        if state not in self.state_filter:
            print("无效状态回调state")
            return
        cur_call_back = self.motor_drive_results[state]
        # 验证是否存在值
        if cur_call_back != None:
            print("回调函数替换")
        else:
            print("回调函数填充")
        # 实际填充
        self.motor_drive_results[state] = callback


    